// (C) Szymon Piechowicz, February 2012

#include "motors.h"
#include "common.h"


#define MOTORS_PWM_L_PORT PORTD
#define MOTORS_PWM_L_PORT_DIR DDRD
#define MOTORS_PWM_L_BIT PD4 // OC1B
#define MOTORS_PWM_R_PORT PORTD
#define MOTORS_PWM_R_PORT_DIR DDRD
#define MOTORS_PWM_R_BIT PD5 // OC1A
#define MOTORS_CONTROL_A_L_PORT PORTC
#define MOTORS_CONTROL_A_L_PORT_DIR DDRC
#define MOTORS_CONTROL_A_L_BIT PC2
#define MOTORS_CONTROL_B_L_PORT PORTC
#define MOTORS_CONTROL_B_L_PORT_DIR DDRC
#define MOTORS_CONTROL_B_L_BIT PC3
#define MOTORS_CONTROL_A_R_PORT PORTC
#define MOTORS_CONTROL_A_R_PORT_DIR DDRC
#define MOTORS_CONTROL_A_R_BIT PC1
#define MOTORS_CONTROL_B_R_PORT PORTC
#define MOTORS_CONTROL_B_R_PORT_DIR DDRC
#define MOTORS_CONTROL_B_R_BIT PC0


// Does nothing if already started.
static void pwm_timer_start(void);
static void pwm_timer_stop(void);


static bool is_pwm_timer_active = false;
static bool are_motors_l_running = false;
static bool are_motors_r_running = false;


void motors_init(void)
{
  PORT_OFF(MOTORS_PWM_L_PORT, MOTORS_PWM_L_BIT);
  PORT_DIR_OUT(MOTORS_PWM_L_PORT_DIR, MOTORS_PWM_L_BIT);
  PORT_OFF(MOTORS_PWM_R_PORT, MOTORS_PWM_R_BIT);
  PORT_DIR_OUT(MOTORS_PWM_R_PORT_DIR, MOTORS_PWM_R_BIT);
  PORT_OFF(MOTORS_CONTROL_A_L_PORT, MOTORS_CONTROL_A_L_BIT);
  PORT_DIR_OUT(MOTORS_CONTROL_A_L_PORT_DIR, MOTORS_CONTROL_A_L_BIT);
  PORT_OFF(MOTORS_CONTROL_B_L_PORT, MOTORS_CONTROL_B_L_BIT);
  PORT_DIR_OUT(MOTORS_CONTROL_B_L_PORT_DIR, MOTORS_CONTROL_B_L_BIT);
  PORT_OFF(MOTORS_CONTROL_A_R_PORT, MOTORS_CONTROL_A_R_BIT);
  PORT_DIR_OUT(MOTORS_CONTROL_A_R_PORT_DIR, MOTORS_CONTROL_A_R_BIT);
  PORT_OFF(MOTORS_CONTROL_B_R_PORT, MOTORS_CONTROL_B_R_BIT);
  PORT_DIR_OUT(MOTORS_CONTROL_B_R_PORT_DIR, MOTORS_CONTROL_B_R_BIT);
  
  TCCR1A |= (1 << WGM10); // phase-correct PWM, 8-bit
}

void motors_l_forward(void)
{
  PORT_OFF(MOTORS_CONTROL_A_L_PORT, MOTORS_CONTROL_A_L_BIT);
  PORT_ON(MOTORS_CONTROL_B_L_PORT, MOTORS_CONTROL_B_L_BIT);
  // Clear OC1B on compare match when counting up, set on compare match when
  // counting down.
  TCCR1A |= (1 << COM1B1);
  are_motors_l_running = true;
  pwm_timer_start();
}

void motors_l_backward(void)
{
  PORT_OFF(MOTORS_CONTROL_B_L_PORT, MOTORS_CONTROL_B_L_BIT);
  PORT_ON(MOTORS_CONTROL_A_L_PORT, MOTORS_CONTROL_A_L_BIT);
  // Clear OC1B on compare match when counting up, set on compare match when
  // counting down.
  TCCR1A |= (1 << COM1B1);
  are_motors_l_running = true;
  pwm_timer_start();
}

void motors_l_stop(void)
{
  PORT_OFF(MOTORS_CONTROL_A_L_PORT, MOTORS_CONTROL_A_L_BIT);
  PORT_OFF(MOTORS_CONTROL_B_L_PORT, MOTORS_CONTROL_B_L_BIT);
  TCCR1A &= ~(1 << COM1B1); // disconnect OC1B
  PORT_OFF(MOTORS_PWM_L_PORT, MOTORS_PWM_L_BIT);
  are_motors_l_running = false;
  if (!are_motors_r_running)
    pwm_timer_stop();
}

void motors_l_set_speed(int8_t percent)
{
  percent = 70 + 30 * percent / 100;
  OCR1B = 255 * percent / 100;
}

void motors_r_forward(void)
{
  PORT_OFF(MOTORS_CONTROL_B_R_PORT, MOTORS_CONTROL_B_R_BIT);
  PORT_ON(MOTORS_CONTROL_A_R_PORT, MOTORS_CONTROL_A_R_BIT);
  // Clear OC1A on compare match when counting up, set on compare match when
  // counting down.
  TCCR1A |= (1 << COM1A1);
  are_motors_r_running = true;
  pwm_timer_start();
}

void motors_r_backward(void)
{
  PORT_OFF(MOTORS_CONTROL_A_R_PORT, MOTORS_CONTROL_A_R_BIT);
  PORT_ON(MOTORS_CONTROL_B_R_PORT, MOTORS_CONTROL_B_R_BIT);
  // Clear OC1A on compare match when counting up, set on compare match when
  // counting down.
  TCCR1A |= (1 << COM1A1);
  are_motors_r_running = true;
  pwm_timer_start();
}

void motors_r_stop(void)
{
  PORT_OFF(MOTORS_CONTROL_A_R_PORT, MOTORS_CONTROL_A_R_BIT);
  PORT_OFF(MOTORS_CONTROL_B_R_PORT, MOTORS_CONTROL_B_R_BIT);
  TCCR1A &= ~(1 << COM1A1); // disconnect OC1A
  PORT_OFF(MOTORS_PWM_R_PORT, MOTORS_PWM_R_BIT);
  are_motors_r_running = false;
  if (!are_motors_l_running)
    pwm_timer_stop();
}

void motors_r_set_speed(int8_t percent)
{
  percent = 70 + 30 * percent / 100;
  OCR1A = 255 * percent / 100;
}

void motors_both_forward(void)
{
  motors_l_forward();
  motors_r_forward();
}

void motors_both_backward(void)
{
 motors_l_backward();
 motors_r_backward();
}

void motors_both_stop(void)
{
  motors_l_stop();
  motors_r_stop();
}

void motors_both_set_speed(int8_t percent)
{
  motors_l_set_speed(percent);
  motors_r_set_speed(percent);
}

static void pwm_timer_start(void)
{
  if (is_pwm_timer_active)
    return;
  TCCR1B |= (1 << CS10); // prescaler / 1
  is_pwm_timer_active = true;
}

static void pwm_timer_stop(void)
{
  TCCR1B &= ~(1 << CS12) & ~(1 << CS10); // no clock, timer stopped
  is_pwm_timer_active = false;
}